﻿
// 2018/8/22: 首个记录，基于V2 2018/8/14
// 2018/11/12: 增加fromGeneralSample
// 2018/11/23: 修正fromGeneralSample时间戳
// 2019/3/29: 添加注释
// 2019/9/26: 车道线颜色扩充
// 2019/9/27: 车道线颜色扩充2
// 2019/10/11: 修改pragma once和include

#ifndef LANE_SENSOR_SAMPLE_V3_H
#define LANE_SENSOR_SAMPLE_V3_H

#include "spadas.h"

namespace lane_sensor_sample_v3
{
	using namespace spadas;

	enum LineClass
	{
		LC_General = 1, // 未指定
		LC_Dashed = 2, // 虚线
		LC_Solid = 3, // 实线
		LC_DoubleDashed = 4, //双虚线
		LC_DoubleSolid = 5, //双实线
		LC_SolidDashed = 6, // 左实右虚
		LC_DashedSolid = 7, // 左虚右实
		LC_Curb = 8, // 路沿
		LC_Guardrail = 9, // 护栏
		LC_Barrier = 10, //障碍物边界
	};

	enum LineColor
	{
		LCR_Unknown = 1, // 未知
		LCR_White = 2, // 白色
		LCR_Yellow = 3, // 黄色
		LCR_Red = 4, // 红色
		LCR_Blue = 5, // 蓝色
		LCR_Orange = 6, // 橙色
		LCR_Green = 7, // 绿色
	};

	struct LaneLine
	{
		// ID相关
		Int id; // 车道线ID
		OptionalInt publicID; // 车道线原始ID

		// 基本信息
		LineClass clas; // 车道线类型
		LineColor color; // 车道线颜色
		OptionalDouble confidence; // 车道线置信度 %
		OptionalDouble width; // 车道线宽度 m

		// 车道线模型
		Double param[4]; // 从0次方到3次方
		OptionalDouble rearEnd; // 曲线后端点位置(x轴) m
		OptionalDouble frontEnd; // 曲线前端点位置(x轴) m

		// 指标
		OptionalDouble dtlc; // m
		OptionalDouble stlc; // m/s
		OptionalDouble ttlc; // s

		LaneLine()
		{
			id = -1;
			clas = LC_General;
			color = LCR_Unknown;
			param[0] = param[1] = param[2] = param[3] = 0;
		}
	};

	struct LaneSensorSample
	{
		GlobalTimestamp timeStamp;// 时间戳

		// 车道线列表
		Array<LaneLine> lines; // 车道线列表
		OptionalInt firstLeftLine; // 左侧第一条车道线序号
		OptionalInt firstRightLine; // 右侧第一条车道线序号
		OptionalInt secondLeftLine; // 左侧第二条车道线序号
		OptionalInt secondRightLine; // 右侧第二条车道线序号

		// 道路属性部分
		OptionalDouble laneWidth; // 当前车道宽度 m
		OptionalDouble laneHeading; // 当前车道朝向 deg
		OptionalDouble laneCurvature; // 当前车道曲率 1/m

		// 本车道行驶状态
		OptionalDouble lateralVelocity; // 当前车道横向速度 m/s
		OptionalDouble centerDeparture; // 当前车道中央偏离距离 m

		// 参考车辆信息
		OptionalDouble vehiVX; // 本车车速 kph
		OptionalDouble vehiCurv; // 本车行驶曲率 1/m
		OptionalDouble vehiAX; // 本车纵向加速度 m/s²
		OptionalDouble vehiWidth; // 本车宽度 m
		OptionalDouble vehiOverhang; // 本车前悬 m

		// 车道线识别边界
		Double rearBound; // 传感器检测范围后边界 m
		Double frontBound; // 传感器检测范围前边界 m

		static String getProtocolName(UInt channel)
		{
			return (String)"lane-sensor-sample-v3@" + channel;
		}

		static String getTitle()
		{
			return "Line count,Lateral velocity[m/s],Center departure[m],Lane width[m],Lane heading[°],Lane curvature[1/m],Vehicle speed[KPH],Vehicle curvature[1/m],Vehicle acceleration[m/s2],Vehicle width[m],Vehicle overhang[m],Rear detect bound[m],Front detect bound[m],First left line index,First right line index,Second left line index,Second right line index,First line's ID,Line's raw ID,Line type,Line color,Line's confidence,Line width,Line's formula order-0,Line's formula order-1,Line's formula order-2,Line's formula order-3,Line's rear end,Line's front end,Line DTLC,Line STLC,Line TTLC,Second line's ID,etc";
		}

		LaneSensorSample() : rearBound(0), frontBound(30)
		{}

		Bool fromGeneralSample(GeneralSample sample)
		{
			Array<OptionalDouble> v = sample.values;

			if (v.size() < 17) return FALSE;
			if (!v[0].valid) return FALSE;

			int lineCount = (int)v[0].value;
			if (v.size() != 17 + lineCount * 15) return FALSE;

			timeStamp = sample.timeStamp;

			lateralVelocity = v[1];
			centerDeparture = v[2];
			laneWidth = v[3];
			laneHeading = v[4];
			laneCurvature = v[5];
			vehiVX = v[6];
			vehiCurv = v[7];
			vehiAX = v[8];
			vehiWidth = v[9];
			vehiOverhang = v[10];
			rearBound = v[11].valid ? v[11].value : 0;
			frontBound = v[12].valid ? v[12].value : 30;
			firstLeftLine = v[13].valid ? (Int)v[13].value : OptionalInt();
			firstRightLine = v[14].valid ? (Int)v[14].value : OptionalInt();
			secondLeftLine = v[15].valid ? (Int)v[15].value : OptionalInt();
			secondRightLine = v[16].valid ? (Int)v[16].value : OptionalInt();

			lines = Array<LaneLine>(lineCount);
			for (int i = 0; i < lineCount; i++)
			{
				LaneLine& line = lines[i];
				UInt baseIndex = 17 + 15 * i;

				line.id = (Int)v[baseIndex + 0].value;
				line.publicID = v[baseIndex + 1].valid ? (Int)v[baseIndex + 1].value : OptionalInt();
				line.clas = (LineClass)(Int)v[baseIndex + 2].value;
				line.color = (LineColor)(Int)v[baseIndex + 3].value;
				line.confidence = v[baseIndex + 4];
				line.width = v[baseIndex + 5];
				line.param[0] = v[baseIndex + 6].value;
				line.param[1] = v[baseIndex + 7].value;
				line.param[2] = v[baseIndex + 8].value;
				line.param[3] = v[baseIndex + 9].value;
				line.rearEnd = v[baseIndex + 10];
				line.frontEnd = v[baseIndex + 11];
				line.dtlc = v[baseIndex + 12];
				line.stlc = v[baseIndex + 13];
				line.ttlc = v[baseIndex + 14];
			}

			return TRUE;
		}
		GeneralSample toGeneralSample()
		{
			GeneralSample sample;
			sample.timeStamp = timeStamp;
			sample.values = Array<OptionalDouble>(17 + lines.size() * 15);
			OptionalDouble *data = sample.values.data();

			sample.values[0] = lines.size();

			sample.values[1] = lateralVelocity;
			sample.values[2] = centerDeparture;
			sample.values[3] = laneWidth;
			sample.values[4] = laneHeading;
			sample.values[5] = laneCurvature;
			sample.values[6] = vehiVX;
			sample.values[7] = vehiCurv;
			sample.values[8] = vehiAX;
			sample.values[9] = vehiWidth;
			sample.values[10] = vehiOverhang;
			sample.values[11] = rearBound;
			sample.values[12] = frontBound;
			sample.values[13] = firstLeftLine.valid ? firstLeftLine.value : OptionalDouble();
			sample.values[14] = firstRightLine.valid ? firstRightLine.value : OptionalDouble();
			sample.values[15] = secondLeftLine.valid ? secondLeftLine.value : OptionalDouble();
			sample.values[16] = secondRightLine.valid ? secondRightLine.value : OptionalDouble();

			for (UInt i = 0; i < lines.size(); i++)
			{
				LaneLine& line = lines[i];
				UInt baseIndex = 17 + i * 15;

				sample.values[baseIndex + 0] = line.id;
				sample.values[baseIndex + 1] = line.publicID.valid ? line.publicID.value : OptionalDouble();
				sample.values[baseIndex + 2] = (Double)(Int)line.clas;
				sample.values[baseIndex + 3] = (Double)(Int)line.color;
				sample.values[baseIndex + 4] = line.confidence;
				sample.values[baseIndex + 5] = line.width;
				sample.values[baseIndex + 6] = line.param[0];
				sample.values[baseIndex + 7] = line.param[1];
				sample.values[baseIndex + 8] = line.param[2];
				sample.values[baseIndex + 9] = line.param[3];
				sample.values[baseIndex + 10] = line.rearEnd;
				sample.values[baseIndex + 11] = line.frontEnd;
				sample.values[baseIndex + 12] = line.dtlc;
				sample.values[baseIndex + 13] = line.stlc;
				sample.values[baseIndex + 14] = line.ttlc;
			}

			sample.significantCount = sample.values.size();
			return sample;
		}
	};
}

#endif